/*  This file is part of YATracker.
    YATracker is free software: you can redistribute it and/or modify it under the terms
of the GNU Lesser General Public License as published by the Free Software Foundation,
either version 3 of the License, or (at your option) any later version.
    YATracker is distributed in the hope that it will be useful, but WITHOUT ANY WARRANTY;
without even the implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.
See the GNU Lesser General Public License for more details.
    You should have received a copy of the GNU Lesser General Public License along with
YATracker. If not, see <http://www.gnu.org/licenses/>. */

#include <Windows.h>
#include <QDebug>
#include <QStringList>
#include "qgpssource.h"

QGpsSource::QGpsSource() :
    QObject()
{
    m_portHandle = (void*)-1;
    m_status = GpsStatusNone;
    m_portReadState = GpsReadStateNone;
    m_portBufferUsed = 0;

    QObject::connect(&m_portReadTimer, SIGNAL(timeout()), this, SLOT(portReadTimerTick()));
}

QGpsSource::~QGpsSource()
{
    gpsClose();
}

void QGpsSource::setComPort(QString portName)
{
    m_portName = portName;
}

GpsStatus QGpsSource::getStatus()
{
    return m_status;
}

bool QGpsSource::gpsConnect()
{
    qDebug() << "gpsConnect()";
    if (m_portName.isEmpty())
        return false;  // Port name not set

    m_portHandle = ::CreateFile((WCHAR*)(m_portName.utf16()), GENERIC_READ, 0, NULL, OPEN_EXISTING, FILE_ATTRIBUTE_NORMAL, 0);
    if (m_portHandle == (void*)-1)
        return false;  // Failed to open the port

    // Set port settings
    DCB dcb;
    //::GetCommState(m_portHandle, &dcb);
    ::memset(&dcb, 0, sizeof(dcb));
    dcb.DCBlength = sizeof(dcb);
    dcb.BaudRate = 0;  //TODO
    dcb.ByteSize = 8;
    dcb.fBinary = 1;
    dcb.fParity = FALSE;
    dcb.fOutxCtsFlow = dcb.fOutxDsrFlow = FALSE;
    dcb.fDtrControl = DTR_CONTROL_DISABLE;
    dcb.fDsrSensitivity = FALSE;
    dcb.fTXContinueOnXoff = FALSE;
    dcb.fOutX = dcb.fInX = FALSE;
    dcb.fErrorChar = FALSE;
    dcb.fNull = FALSE;
    dcb.fRtsControl = RTS_CONTROL_DISABLE;
    dcb.fAbortOnError = FALSE;
    dcb.Parity = NOPARITY;
    dcb.StopBits = ONESTOPBIT;
    if (!::SetCommState(m_portHandle, &dcb))
        qDebug() << "gpsConnect() failed SetCommState call";

    // Set timeouts: ReadIntervalTimeout value of MAXDWORD, combined with zero values for both the ReadTotalTimeoutConstant
    // and ReadTotalTimeoutMultiplier members, specifies that the read operation is to return immediately with the bytes
    // that have already been received, even if no bytes have been received.
    COMMTIMEOUTS timeouts;
    timeouts.ReadIntervalTimeout = MAXDWORD;
    timeouts.ReadTotalTimeoutConstant = 0;
    timeouts.ReadTotalTimeoutMultiplier = 0;
    if (!::SetCommTimeouts(m_portHandle, &timeouts))
        qDebug() << "gpsConnect() failed SetCommTimeouts call";

    // Clear port input buffer
    ::PurgeComm(m_portHandle, PURGE_RXABORT|PURGE_RXCLEAR);

    // Prepare read buffer
    m_portBufferUsed = 0;
    m_portReadState = GpsReadStateWaitStart;

    m_portReadTimer.setInterval(333);  //TODO
    m_portReadTimer.start();

    m_status = GpsStatusConnect;
    return true;
}

void QGpsSource::gpsClose()
{
    qDebug() << "gpsClose()";
    if (m_portHandle != (void*)-1)
        ::CloseHandle(m_portHandle);
    m_portHandle = (void*)-1;

    m_status = GpsStatusNone;
    m_portReadState = GpsReadStateNone;
    m_portBufferUsed = 0;

    m_portReadTimer.stop();
}

void QGpsSource::portReadTimerTick()
{
    //qDebug() << "portReadTimerTick()";
    if (m_status == GpsStatusNone)
        return;
    if (m_portReadState == GpsReadStateError)
    {
        // Reset reading state after an error
        m_portBufferUsed = 0;
        m_portReadState = GpsReadStateWaitStart;
    }

    qint64 bytesToRead = sizeof(m_portBuffer) - m_portBufferUsed;
    if (bytesToRead <= 0)  // Read buffer is overfull
    {
        m_portReadState = GpsReadStateError;
        return;
    }

    // Read the data
    qint32 bytesRead;
    if (!::ReadFile(m_portHandle, m_portBuffer + m_portBufferUsed, bytesToRead, (DWORD*)&bytesRead, NULL))
    {
        //qDebug() << "portFileReadyRead() read error";
        m_portReadState = GpsReadStateError;
        return;
    }
    if (bytesRead > 0)
    {
        //qDebug() << "portFileReadyRead() read " << bytesRead;
        m_portBufferUsed += bytesRead;

        parsePortBuffer();
    }
}

void QGpsSource::parsePortBuffer()
{
    //qDebug() << "parsePortBuffer()";
    if (m_portBufferUsed == 0)
        return;
    if (m_portReadState == GpsReadStateNone)
        return;

    while (true)
    {
        if (m_portReadState == GpsReadStateWaitStart)
        {
            // Search for '$' sign
            qint32 posDollar = -1;
            for (int i = 0; i < m_portBufferUsed; i++)
            {
                if (m_portBuffer[i] == '$')
                {
                    posDollar = i;
                    break;
                }
            }
            if (posDollar == -1)  // '$' sign not found
            {
                if (m_portBufferUsed > (qint32)(sizeof(m_portBuffer) / 2))  // Wrong data
                {
                    m_portReadState = GpsReadStateError;
                    return;
                }

                return;  // Wait for a next round
            }
            else
            {
                if (posDollar > 0)
                {
                    // Move the message to the start of the buffer
                    ::memmove(m_portBuffer, m_portBuffer + posDollar, m_portBufferUsed - posDollar);
                    m_portBufferUsed -= posDollar;
                }
                m_portReadState = GpsReadStateWaitEnd;
            }
        }

        if (m_portReadState == GpsReadStateWaitEnd)
        {
            // Search for end-of-line sign
            qint32 posCR = -1;
            for (int i = 0; i < m_portBufferUsed; i++)
            {
                if (m_portBuffer[i] == 13)
                {
                    posCR = i;
                    break;
                }
            }

            if (posCR == -1)
            {
                if (m_portBufferUsed > (qint32)(sizeof(m_portBuffer) / 4 * 3))  // Wrong data
                {
                    m_portReadState = GpsReadStateError;
                    return;
                }

                return;  // Wait for a next round
            }
            else
            {
                m_portBuffer[posCR] = 0;
                parseMessage(m_portBuffer);

                if (posCR > 0)
                {
                    // Move the buffer tail to the start of the buffer
                    ::memmove(m_portBuffer, m_portBuffer + posCR, m_portBufferUsed - posCR);
                    m_portBufferUsed -= posCR;
                }
                m_portReadState = GpsReadStateWaitStart;
            }
        }
    }
}

void QGpsSource::parseMessage(QString message)
{
    //qDebug() << "parseMessage(): " << message;
    if (message[0] != '$')  // Skip non-NMEA messages
        return;
    if (message[1] != 'G' || message[2] != 'P')  // Skip non-GPS messages
        return;

    // Check if we have a checksum at the end of the message
    if (message.length() > 4 && message[message.length() - 3] == '*')
    {
        //TODO: Calculate and verify checksum
        message.remove(message.length() - 3, 3);
    }

    message.remove(0, 1);  // Remove leading '$'
    QStringList tokens = message.split(',');
    if (tokens.length() == 0)
        return;  // Strange empty message

    QString scode = tokens.at(0);
    if (scode.compare("GPRMC") == 0)  // Recommended minimum specific GPS/Transit data
    {
        if (tokens.length() < 12 || tokens[3].length() < 3 || tokens[5].length() < 4)
            return;
        double latitude = tokens[3].left(2).toDouble() + (tokens[3].mid(2).toDouble() / 60.0);
        if (tokens[4].compare("S") == 0)
            latitude *= -1.0;
        double longitude = tokens[5].left(3).toDouble() + (tokens[5].mid(3).toDouble() / 60.0);
        if (tokens[6].compare("W") == 0)
            longitude *= -1.0;

        qDebug() << "parseMessage() GPRMC " << latitude << " / " << longitude;
        emit gpsMessageRMC(latitude, longitude);
    }
    else if (scode.compare("GPGSA") == 0)  // GPS DOP and active satellites
    {
        if (tokens.length() < 18 || tokens[15].length() < 1 || tokens[16].length() < 1 || tokens[17].length() < 1)
            return;
        int fix = tokens[2].toInt();
        float pdop = tokens[15].toFloat();
        float hdop = tokens[16].toFloat();
        float vdop = tokens[17].toFloat();
        // Calculate number of satellites used in position fix
        int fixSats = 0;
        for (int i = 3; i <= 14; i++)
            if (tokens[i].length() > 0) fixSats++;

        qDebug() << "parseMessage() GPGSA " << fix << " " << pdop << " / " << hdop << " / " << vdop;
        emit gpsMessageGSA(fix, pdop, hdop, vdop, fixSats);
    }
    else if (scode.compare("GPGGA") == 0)  // Global Positioning System Fix Data
    {
        if (tokens.length() < 15 || tokens[2].length() == 0 || tokens[4].length() == 0)
            return;
        double latitude = tokens[2].left(2).toDouble() + (tokens[2].mid(2).toDouble() / 60.0);
        if (tokens[3].compare("S") == 0)
            latitude *= -1.0;
        double longitude = tokens[4].left(3).toDouble() + (tokens[4].mid(3).toDouble() / 60.0);
        if (tokens[5].compare("W") == 0)
            longitude *= -1.0;
        int fix = tokens[6].toInt();
        int fixsats = tokens[7].toInt();

        qDebug() << "parseMessage() GPGGA " << fix << " " << latitude << " / " << longitude << ", " << fixsats;
        emit gpsMessageGGA(fix, latitude, longitude, fixsats);
    }
}
